#include "inc/hw_types.h"
#include "uart.h"
#include "OS.h"
#include "IR.h"
#include "FIFO.h"
#include "rit128x96x4.h"
#include "adc_c.h"
#include "can.h"
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

_tFIFO DistanceLeft,DistanceFront, DistanceRight;
unsigned long filter1Array[4] = {0,0,0,0};
unsigned long filter2Array[4] = {0,0,0,0};
unsigned long filter3Array[4] = {0,0,0,0};

void IR_Init(void)
{
  DistanceLeft = FIFO_CreateFifo(32, sizeof(unsigned long));
  DistanceFront = FIFO_CreateFifo(32, sizeof(unsigned long));
  DistanceRight = FIFO_CreateFifo(32, sizeof(unsigned long));
}

void IRDataCollect(OS_TCB* this)
{
  //char buf[30];
  unsigned long dataIRLeft;
  unsigned long dataIRFront;
  unsigned long dataIRRight;
  //unsigned long dataSendLeft, dataSendFront, dataSentRight; 
  //int i = 0;
  //int counter = 0;
  //int device = 0;
  IR_Init();
  while(1){
    dataIRLeft  = ADC_In(0);
    dataIRFront = ADC_In(1);
    dataIRRight = ADC_In(2);
	dataIRLeft  = filter1(dataIRLeft);
	dataIRFront = filter2(dataIRFront);
	dataIRRight = filter3(dataIRRight);
//	if(dataIRLeft >= 1023)
//	{
//	  dataSendLeft = 1;
//	  dataSendFront = 1;
//	}
//	else
//	{
//	  dataSendLeft = (int)((17570*pow((double)dataSendLeft, -1.272))*100);
//	  dataSendFront = (int)((17570*pow((double)dataSendFront, -1.272))*100);
//	}

	FIFO_PutSingle(DistanceLeft, &dataIRLeft);
	FIFO_PutSingle(DistanceFront, &dataIRFront);
	FIFO_PutSingle(DistanceRight, &dataIRRight);

	//i++;
	OS_Sleep(this,30); //Need this thread to sleep for 30 ms
  }
}

_tFIFO Ping	;
unsigned char PingDistance ;
void PingReceiveInit(void){
	 Ping = FIFO_CreateFifo(32, sizeof(unsigned char));
	CAN_Init();
}

void PingReceive(OS_TCB* this)
{
	int i;
	unsigned char data;
	unsigned long pingData;
	  PingReceiveInit();
	  while(1){
		//get samples
		i=0; 
		pingData = 0;
		while(0 == FIFO_GetSingle(Ping, &data)){
			i++;
			pingData += data;
		}
		if(i == 0){
			//IRError++;
			;
			
		}
		else {
			PingDistance = pingData;
		}
		OS_Sleep(this,100);   //run at 10hz
	  }

}

unsigned long filter1(unsigned long data)
{
  int a;
  unsigned long filteredData;
  for( a = 2; a>=0; a--)
  {
    filter1Array[a+1] = filter1Array[a];
  }
  filter1Array[0] = data;
  filteredData = ((filter1Array[0]+filter1Array[3])>>1);
  return filteredData;
}

unsigned long filter2(unsigned long data)
{
  int a;
  unsigned long filteredData;
  for( a = 2; a>=0; a--)
  {
    filter2Array[a+1] = filter2Array[a];
  }
  filter2Array[0] = data;
  filteredData = ((filter2Array[0]+filter2Array[3])>>1);
  return filteredData;
}

unsigned long filter3(unsigned long data)
{
  int a;
  unsigned long filteredData;
  for( a = 2; a>=0; a--)
  {
    filter3Array[a+1] = filter3Array[a];
  }
  filter3Array[0] = data;
  filteredData = ((filter3Array[0]+filter3Array[3])>>1);
  return filteredData;
}








